clear;
clc;
%% 定义小车倒立摆物理性质
R = 0.07625; %车轮的半径
D = 0.31; %左轮、右轮两个轮子间的距离
l = 0.05; %摆杆质心到转轴距离
m = 0.741 * 2; %车轮的质量, 共轴麦轮为两个轮的质量
M = 4.886; %摆杆质量
I = (1/2)*m*R^2; %车轮的转动惯量
Jy = (1/3)*M*l^2; %机器人机体对 y 轴的运动时产生的转动惯量(俯仰方向)
Jz = (1/12)*M*D^2; %机器人机体对 z 轴的运动时产生的转动惯量(偏航方向)
g = 9.8; %重力加速度

%% 状态空间矩阵
Q_eq = Jy*M + (Jy+M*l^2) * (2*m+(2*I)/R^2);
% A为系统矩阵
A_23=-(M^2*l^2*g)/Q_eq;
A_43=M*l*g*(M+2*m+(2*I/R^2))/Q_eq;
A = [0 1 0 0 0 0;
     0 0 A_23 0 0 0;
     0 0 0 1 0 0;
     0 0 A_43 0 0 0;
     0 0 0 0 0 1;
     0 0 0 0 0 0;];
% B为输入矩阵
B_21=(Jy+M*l^2+M*l*R)/Q_eq/R;
B_22 = B_21;
B_41=-((M*l/R)+M+2*m+(2*I/R^2))/Q_eq;
B_42 = B_41;
B_61 = -D/(D^2*(M*R+I/R)+2*Jz*R);
B_62 = -B_61;
B = [0 0;
     B_21 B_22;
     0 0;
     B_41 B_42;
     0 0;
     B_61 B_62];
% C为输出矩阵
C = eye(6);
D = 0;
%% 求LQR增益矩阵
Q = diag([0.001 4 8 0.8 4 0.8]);
R = diag([1 1]);
K = lqr(A, B,Q,R);
disp('K = ');
disp(K);

